#include "csgsolidnode.h"
#include "expression/bernstein.hpp"
#include "bezier.h"

#include "GL/glu.h"

CSGSolidNode::CSGSolidNode()
    : CSGNode(0), Bezier()
{
    solidPts = NULL;

    numOfSolidPts = 1;
    for (int i = 0; i < dimSpace; i++)
    {
        dimSolidPts[i] = 15;
        numOfSolidPts *= dimSolidPts[i];
    }
}

CSGSolidNode::CSGSolidNode(int *_ctrlPoly, double **_ctrlPts, CSGNode *parent)
    : CSGNode(0, parent), Bezier(3, 3, _ctrlPoly, _ctrlPts)
{
    solidPts = NULL;

    numOfSolidPts = 1;
    for (int i = 0; i < dimSpace; i++)
    {
        dimSolidPts[i] = 15;
        numOfSolidPts *= dimSolidPts[i];
    }
}

CSGSolidNode::~CSGSolidNode()
{
    if (solidPts)
    {
        for (int i = 0; i < numOfSolidPts; i++)
        {
            delete []solidPts[i];
        }
        delete []solidPts;
    }
}

int CSGSolidNode::getNumOfCtrlPts()
{
    return Bezier::getNumOfCtrlPts();
}

void CSGSolidNode::drawBoundingBox()
{
    glBegin(GL_LINE_LOOP);
    glVertex3d(boundingBox.x0, boundingBox.y0, boundingBox.z0);
    glVertex3d(boundingBox.x0, boundingBox.y1, boundingBox.z0);
    glVertex3d(boundingBox.x1, boundingBox.y1, boundingBox.z0);
    glVertex3d(boundingBox.x1, boundingBox.y0, boundingBox.z0);
    glEnd();

    glBegin(GL_LINE_LOOP);
    glVertex3d(boundingBox.x0, boundingBox.y0, boundingBox.z1);
    glVertex3d(boundingBox.x0, boundingBox.y1, boundingBox.z1);
    glVertex3d(boundingBox.x1, boundingBox.y1, boundingBox.z1);
    glVertex3d(boundingBox.x1, boundingBox.y0, boundingBox.z1);
    glEnd();

    glBegin(GL_LINES);
    glVertex3d(boundingBox.x0, boundingBox.y0, boundingBox.z0);
    glVertex3d(boundingBox.x0, boundingBox.y0, boundingBox.z1);
    glVertex3d(boundingBox.x0, boundingBox.y1, boundingBox.z0);
    glVertex3d(boundingBox.x0, boundingBox.y1, boundingBox.z1);
    glVertex3d(boundingBox.x1, boundingBox.y1, boundingBox.z0);
    glVertex3d(boundingBox.x1, boundingBox.y1, boundingBox.z1);
    glVertex3d(boundingBox.x1, boundingBox.y0, boundingBox.z0);
    glVertex3d(boundingBox.x1, boundingBox.y0, boundingBox.z1);
    glEnd();
}

void CSGSolidNode::drawControlPolygon(bool fill)
{
    if (fill)
    {
        drawMesh(dimCtrlPoly, ctrlPts);
    }
    else
    {
        drawOctahedra(dimCtrlPoly, ctrlPts);
    }
}

void CSGSolidNode::drawNaively(bool grid)
{
    if (solidPts)
    {
        drawMesh(dimSolidPts, solidPts);
    }

    if (grid)
    {
        glDisable(GL_LIGHTING);
        glColor3f(0.2, 0.2, 0.2);
        drawOctahedra(dimSolidPts, solidPts);
    }
}

void CSGSolidNode::drawCasteljau()
{
    drawCasteljau(*this, 3);
}

void CSGSolidNode::drawDomain(bool grid)
{

}

void CSGSolidNode::computeBoundingBox()
{
    if (ctrlPts == NULL)
    {
        boundingBox.x0 = 0;
        boundingBox.x1 = 0;
        boundingBox.y0 = 0;
        boundingBox.y1 = 0;
        boundingBox.z0 = 0;
        boundingBox.z1 = 0;
        return;
    }

    boundingBox.x0 = boundingBox.x1 = ctrlPts[0][0];
    boundingBox.y0 = boundingBox.y1 = ctrlPts[0][1];
    boundingBox.z0 = boundingBox.z1 = ctrlPts[0][2];

    for (int i = 0; i < numCtrlPts; i++)
    {
        if (boundingBox.x0 > ctrlPts[i][0])
        {
            boundingBox.x0 = ctrlPts[i][0];
        }
        if (boundingBox.x1 < ctrlPts[i][0])
        {
            boundingBox.x1 = ctrlPts[i][0];
        }

        if (boundingBox.y0 > ctrlPts[i][1])
        {
            boundingBox.y0 = ctrlPts[i][1];
        }
        if (boundingBox.y1 < ctrlPts[i][1])
        {
            boundingBox.y1 = ctrlPts[i][1];
        }

        if (boundingBox.z0 > ctrlPts[i][2])
        {
            boundingBox.z0 = ctrlPts[i][2];
        }
        if (boundingBox.z1 < ctrlPts[i][2])
        {
            boundingBox.z1 = ctrlPts[i][2];
        }
    }
}

bool CSGSolidNode::solve(StarTestBox *box, double **patchCtrlPts, int k, double **curveCtrlPts, double x, double y)
{

}

bool CSGSolidNode::complementSolve(StarTestBox *box, double **patchCtrlPts, int k, double **curveCtrlPts, double x, double y)
{

}

void CSGSolidNode::generateSolidPoints()
{
    if (ctrlPts == NULL)
    {
        return;
    }

    int index_pt;

    if (solidPts != NULL)
    {
        for (int i = 0; i < numOfSolidPts; i++)
        {
            delete []solidPts[i];
        }

        delete []solidPts;
        solidPts = NULL;
    }

    double step_u = 1.0 / (dimSolidPts[0] - 1);
    double step_v = 1.0 / (dimSolidPts[1] - 1);
    double step_w = 1.0 / (dimSolidPts[2] - 1);

    solidPts = new double *[numOfSolidPts];

    for (int i = 0; i < numOfSolidPts; i++)
    {
        solidPts[i] = new double[DIM_SPACE];
    }

    double u, v, w;

    for (int k = 0; k < dimSolidPts[2]; k++)
    {
        w = k * step_w;

        for (int j = 0; j < dimSolidPts[1]; j++)
        {
            v = j * step_v;

            for (int i = 0; i < dimSolidPts[0]; i++)
            {
                u = i * step_u;

                int index[3] = {i, j, k};
                index_pt = ptIndex(index, dimSolidPts);

                evalPoint(u, v, w, solidPts[index_pt]);
            }
        }
    }
}

void CSGSolidNode::evalPoint(double u, double v, double w, double *pt)
{
    double u0 = 0, u1 = 1;
    double v0 = 0, v1 = 1;
    double w0 = 0, w1 = 1;

    ::numeric::Interval<double>::self domain_u = ::numeric::Hull(u0, u1);
    ::expression::Variable uu("u", domain_u);
    ::expression::Expression expr_u = uu.getSymbol();
    ::numeric::Interval<double>::self domain_v = ::numeric::Hull(v0, v1);
    ::expression::Variable vv("v", domain_v);
    ::expression::Expression expr_v = vv.getSymbol();
    ::numeric::Interval<double>::self domain_w = ::numeric::Hull(w0, w1);
    ::expression::Variable ww("w", domain_w);
    ::expression::Expression expr_w = ww.getSymbol();

    ::expression::Expression * exprBlock = expSolid(dimCtrlPoly, ctrlPts, expr_u, expr_v, expr_w);

    ::expression::sym_mapper<double>::type var_map;

    var_map.insert(std::pair<expression::Expression, double>(expr_u, u));
    var_map.insert(std::pair<expression::Expression, double>(expr_v, v));
    var_map.insert(std::pair<expression::Expression, double>(expr_w, w));

    pt[0] = ::expression::Eval<double>(exprBlock[0], var_map);
    pt[1] = ::expression::Eval<double>(exprBlock[1], var_map);
    pt[2] = ::expression::Eval<double>(exprBlock[2], var_map);

    delete []exprBlock;
}

void CSGSolidNode::setRotation(double _rot)
{

}

void CSGSolidNode::setTranslation(double x, double y)
{
    for (int i = 0; i < numCtrlPts; i++)
    {
        ctrlPts[i][0] += x;
        ctrlPts[i][1] += y;
    }

    computeBoundingBox();
    generateSolidPoints();
}

void CSGSolidNode::setScale(double x, double y)
{
    computeBoundingBox();
    double cpt[2] = {(boundingBox.x0 + boundingBox.x1)/2, (boundingBox.y0 + boundingBox.y1)/2};
    double vec[2];

    double ** newCtrlPts = new double*[numCtrlPts];
    for (int i = 0; i < numCtrlPts; i++)
    {
        newCtrlPts[i] = new double[DIM_SPACE];
        for (int j = 0; j < DIM_SPACE; j++)
        {
            newCtrlPts[i][j] = ctrlPts[i][j];
        }
    }

    for (int i = 0; i < numCtrlPts; i++)
    {
        newCtrlPts[i][0] = (ctrlPts[i][0] - cpt[0]) * x;
        newCtrlPts[i][1] = (ctrlPts[i][1] - cpt[1]) * y;
    }
    for (int i = 0; i < numCtrlPts; i++)
    {
        newCtrlPts[i][0] = vec[0] + cpt[0];
        newCtrlPts[i][1] = vec[1] + cpt[1];
    }

    releaseCtrlPts();
    ctrlPts = newCtrlPts;

    computeBoundingBox();
    generateSolidPoints();
}

void CSGSolidNode::initial(StarTestBox &box, int dim)
{
    computeBoundingBox();
    generateSolidPoints();
}

bool CSGSolidNode::emptyTest(StarTestBox *box)
{

}

bool CSGSolidNode::starTest(StarTestBox *box, double *pt)
{

}

bool CSGSolidNode::complementStarTest(StarTestBox *box, double *pt)
{

}

bool CSGSolidNode::ptInSet(double *pt)
{

}

void CSGSolidNode::criticalTest(StarTestBox *paramBox)
{

}

bool CSGSolidNode::curveIntersectSegment(int k, double **curveCtrlPts, double x, double y, StarTestBox *box)
{

}

bool CSGSolidNode::bdAgainstStar(double x, double y, StarTestBox *box, int bound)
{

}

bool CSGSolidNode::bdAgainstComplementStar(double x, double y, StarTestBox *box, int bound)
{

}

bool CSGSolidNode::intersectBoundingBox(StarTestBox *box)
{

}

bool CSGSolidNode::intersectBox(StarTestBox *box)
{

}

bool CSGSolidNode::boxIntersectBoundary(StarTestBox *box)
{

}

bool CSGSolidNode::boxIntersectBoundaryCurve(StarTestBox *box, int bound)
{

}

bool CSGSolidNode::boxContainsCriticalPt(StarTestBox *box)
{

}

bool CSGSolidNode::starCondition(StarTestBox *box, double *pt)
{

}

bool CSGSolidNode::complementStarCondition(StarTestBox *box, double *pt)
{

}

Expression *CSGSolidNode::expSolid(int *dim, double **pt_array,
                                   Expression &expr_u, Expression &expr_v, Expression &expr_w)
{
    expression::BernsteinPolGenerator bernsteinGen;
    Expression * expr_sum;

    expr_sum = new Expression[DIM_SPACE];
    int ai = 0;
    // B00P00
    for (ai = 0; ai < DIM_SPACE; ai++)
    {
        expr_sum[ai] = pt_array[0][ai]
                * bernsteinGen.generate(0, dim[0], expr_u)
                * bernsteinGen.generate(0, dim[1], expr_v)
                * bernsteinGen.generate(0, dim[2], expr_w);
    }

    // sum(BijkPijk)
    double *pt;
    for (int i = 0; i < dim[0]; i++)
    {
        for (int j = 0; j < dim[1]; j++)
        {
            for (int k = 0; k < dim[2]; k++)
            {
                if (i == 0 && j == 0 && k == 0)
                {
                    continue;
                }

                int index[3] = {i, j, k};
                pt = getPt(index, dim, pt_array);

                for (ai = 0; ai < DIM_SPACE; ai++)
                {
                    expr_sum[ai] += pt[ai]
                                * bernsteinGen.generate(i, dim[0]-1, expr_u)
                                * bernsteinGen.generate(j, dim[1]-1, expr_v)
                                * bernsteinGen.generate(k, dim[2]-1, expr_w);
                }
            }
        }
    }

    return expr_sum;
}

void CSGSolidNode::drawMesh(int row, int col, double **meshPts, bool bPolygon)
{

}

void CSGSolidNode::drawCasteljau(CSGSolidNode &solid, int level)
{
    if (level == 0)
    {
        solid.drawControlPolygon(true);
    }
    else
    {
        CSGSolidNode tempSubSolid[4] =
        {
            CSGSolidNode(dimCtrlPoly, ctrlPts), CSGSolidNode(dimCtrlPoly, ctrlPts),
            CSGSolidNode(dimCtrlPoly, ctrlPts), CSGSolidNode(dimCtrlPoly, ctrlPts)
        };

        CSGSolidNode subSolid[8] =
        {
            CSGSolidNode(dimCtrlPoly, ctrlPts), CSGSolidNode(dimCtrlPoly, ctrlPts),
            CSGSolidNode(dimCtrlPoly, ctrlPts), CSGSolidNode(dimCtrlPoly, ctrlPts),
            CSGSolidNode(dimCtrlPoly, ctrlPts), CSGSolidNode(dimCtrlPoly, ctrlPts),
            CSGSolidNode(dimCtrlPoly, ctrlPts), CSGSolidNode(dimCtrlPoly, ctrlPts)
        };

        solid.casteljau(0, 0.5, subSolid, subSolid+1);

        subSolid[0].casteljau(1, 0.5, tempSubSolid, tempSubSolid+1);
        subSolid[1].casteljau(1, 0.5, tempSubSolid+2, tempSubSolid+3);

        for (int i = 0; i < 4; i++)
        {
            tempSubSolid[i].casteljau(2, 0.5, subSolid + i*2, subSolid + i*2 + 1);
        }

        --level;
        for (int i = 0; i < 8; i++)
        {
            drawCasteljau(subSolid[i], level);
        }
    }
}

void CSGSolidNode::criticalTest(TreeNode<StarTestBox> *node, int M, int N, double **jCtrlPts)
{

}

void CSGSolidNode::releaseCtrlPts()
{
    for (int i = 0; i < numCtrlPts; i++)
    {
        delete []ctrlPts[i];
    }
    delete []ctrlPts;
}

void CSGSolidNode::releaseCtrlPts(int m, int n, int d, double **ctrlPts)
{

}

void CSGSolidNode::drawOctahedra(int *dim, double **pts)
{
    double *pt;
    for (int i = 0; i < dim[0]; i++)
    {
        for (int j = 0; j < dim[1]; j++)
        {
            for (int k = 0; k < dim[2]; k++)
            {
                int index[3] = {i, j, k};
                int nIndex[3] = {i, j, k};

                for (int l = 0; l < 3; l++)
                {
                    if (++nIndex[l] <= dim[l])
                    {
                        glBegin(GL_LINES);
                        pt = getPt(index, dim, pts);
                        if (pt)
                        {
                            glVertex3d(pt[0], pt[1], pt[2]);
                        }
                        pt = getPt(nIndex, dim, pts);
                        if (pt)
                        {
                            glVertex3d(pt[0], pt[1], pt[2]);
                        }
                        glEnd();
                    }
                    --nIndex[l];
                }
            }
        }
    }
}

void CSGSolidNode::drawMesh(int *dim, double **pts)
{
    double *pt[8];
    for (int i = 0; i < dim[0]; i++)
    {
        for (int j = 0; j < dim[1]; j++)
        {
            for (int k = 0; k < dim[2]; k++)
            {
                int index1[8][3] = {{i, j, k}, {i+1, j, k}, {i+1, j+1, k}, {i, j+1, k},
                                   {i, j, k+1}, {i+1, j, k+1}, {i+1, j+1, k+1}, {i, j+1, k+1}};
                for (int l = 0; l < 8; l++)
                {
                    pt[l] = getPt(index1[l], dim, pts);
                }
                drawCube(pt);
            }
        }
    }
}

void CSGSolidNode::drawCube(double **pt)
{
    double c[3] = {0, 0, 0};
    for (int i = 0; i < 8; i++)
    {
        if (pt[i] == NULL)
        {
            return;
        }
        else
        {
            for (int j = 0; j < 3; j++)
            {
                c[j] += pt[i][j]/8;
            }
        }
    }

    drawRect(pt[0], pt[1], pt[2], pt[3], c);
    drawRect(pt[0], pt[4], pt[5], pt[1], c);
    drawRect(pt[0], pt[3], pt[7], pt[4], c);
    drawRect(pt[2], pt[1], pt[5], pt[6], c);
    drawRect(pt[5], pt[4], pt[7], pt[6], c);
    drawRect(pt[7], pt[3], pt[2], pt[6], c);
}

void CSGSolidNode::drawRect(double *p1, double *p2, double *p3, double *p4, double *c)
{
    double cp[3] = {0, 0, 0};

    for (int i = 0; i < 3; i++)
    {
        cp[i] += (p1[i] + p2[i] + p3[i] + p4[i])/4;
    }

    double normal[3];
    for (int i = 0; i < 3; i++)
    {
        normal[i] = cp[i] - c[i];
    }

    glBegin(GL_POLYGON);
    glNormal3dv(normal);
    glVertex3dv(p1);
    glVertex3dv(p2);
    glVertex3dv(p3);
    glVertex3dv(p4);
    glEnd();
}
